#include "imu.h"
#include <stdio.h>
#include <iostream>
#include <sstream>

using namespace std;



Imu::Imu()
{     
  char* serial = "IMU";
      
  correct[0] = .0109863;
  correct[1] = .0109863;
  correct[2] = .0109863;
  correct[3] = .0137329;
  correct[4] = .0137329;
  correct[5] = .0137329;    
  correct[6] = .061035;
  correct[7] = .061035;
  correct[8] = .061035;    
  correct[9] = .01812;
  correct[10] = .01812;
  correct[11] = .01812;  
  correct[12] = (.106812/1000)*9.8;
  correct[13] = (.106812/1000)*9.8;
  correct[14] = (.106812/1000)*9.8;
  
  ftdi = new Serial(serial);
 
  disableMagCorrection();
  ekfReset();
  setActiveChannels(0xFF,0xFE);
  autoSetAccelRef();
  zeroRateGyros();
  selfTest();
  setListen();

  readChannels();
}

void Imu::readChannels()
{
  getData();
  unsigned char buffer[1024];
  int status = readPacket(buffer);
  
  if (status == -1)
    return;
    
  int PT = buffer[0];
  
  if(PT == 0xB7)
  {
    //fprintf(stderr,"Active Channels: %x%x\n",buffer[2],buffer[3]);
    for(int i = 0; i < 15; i++)
    {
      unsigned short rawData = (buffer[4+2*i]<<8)^(buffer[5+2*i]);
      short realData = rawData;
      double corrected = realData*correct[i];
      imuData[i] = corrected;
    }
  }
  else
  {
    printResponse(PT,buffer);
  }
  
  if(imuData[0] < 0)
    imuData[0] += 360;
  
  for(int i = 1; i < 3; i++)
  {
    if(imuData[i] > 180)
      imuData[i] -= 360;
    else if(imuData[i] < -180)
      imuData[i] += 360;  
  }
  
  if(imuData[0] == 0)
    ekfReset();  
    
//  double totalg = sqrt(pow(imuData[12],2)+pow(imuData[13],2)+pow(imuData[14],2));
//  fprintf(stderr,"%f\n",totalg);
}

void Imu::printResponse(int PT,unsigned char *buffer)
{
  switch(PT)
  {
    case 0xB0:
      fprintf(stderr,"Command Complete\n");
      break;
    case 0xB1:
      fprintf(stderr,"Command Failed\n");
      break;
    case 0xB2:
      fprintf(stderr,"Bad Checksum\n");
      break;
    case 0xB3:
      fprintf(stderr,"Bad Data Length\n");
      break;
    case 0xB4:
      fprintf(stderr,"Unrecognized Packet\n");
      break;
    case 0xB5:
      fprintf(stderr,"Buffer Overflow\n");
      break;
    case 0xB6:
      fprintf(stderr,"Status Report:\n");
      fprintf(stderr,"%x\n",buffer[2]);
      break;
    case 0xB7:
      fprintf(stderr,"Sensor Data\n");
      break;     
    case 0xB8:
      fprintf(stderr,"Gyro Bias Report\n");
      break;
    case 0xB9:
      fprintf(stderr,"Gyro Scale Report\n");
      break;  
    case 0xBA:
      fprintf(stderr,"START_CAL_REPORT\n");
      break;
    case 0xBB:
      fprintf(stderr,"ACCEL_BIAS_REPORT\n");
      break;
    case 0xBC:
      fprintf(stderr,"ACCEL_REF_VECTOR_REPORT:\n");
      fprintf(stderr,"z: %x%x y: %x%x x:%x%x\n",buffer[2],buffer[3],buffer[4],buffer[5],buffer[6],buffer[7]);
      break;
    case 0xBD:
      fprintf(stderr,"ACTIVE_CHANNEL_REPORT\n");
      break;
    case 0xBE:
      fprintf(stderr,"ACCEL_COVARIANCE_REPORT\n");
      break;
    case 0xBF:
      fprintf(stderr,"MAG_COVARIANCE_REPORT\n");
      break;
    case 0xC0:
      fprintf(stderr,"PROCESS_COVARIANCE_REPORT\n");
      break;
    case 0xC1:
      fprintf(stderr,"STATE_COVARIANCE_REPORT\n");
      break;
    case 0xC2:
      fprintf(stderr,"EKF_CONFIG_REPORT\n");
      break;
    case 0xC3:
      fprintf(stderr,"GYRO_ALIGNMENT_REPORT\n");
      break;
    case 0xC4:
      fprintf(stderr,"ACCEL_ALIGNMENT_REPORT\n");
      break;
    case 0xC5:
      fprintf(stderr,"MAG_REF_VECTOR_REPORT\n");
      break;
    case 0xC6:
      fprintf(stderr,"MAG_CAL_REPORT\n");
      break;
    case 0xC7:
      fprintf(stderr,"MAG_BIAS_REPORT\n");
      break;
    case 0xC8:
      fprintf(stderr,"BROADCAST_MODE_REPORT\n");
      break;
  }
  
  if(PT < 0xB0 || PT > 0xC8)
    fprintf(stderr,"%x\n",PT);
}

double Imu::getIMUData(int index)
{
  return imuData[index];
}

void Imu::setListen()
{
  unsigned char writeBuffer[7] = {'s','n','p',0x81,0,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 5; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[5] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[6] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 7);

  if(retVal != -1)
    waitResponse();
}

void Imu::getData()
{
  unsigned char writeBuffer[7] = {'s','n','p',0x01,0,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 5; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[5] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[6] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 7);

  //if(retVal != -1)
    //waitResponse();
}

void Imu::setActiveChannels(char d1, char d2)
{
  unsigned char writeBuffer[9] = {'s','n','p',0x80,2,d1,d2,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 7; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[7] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[8] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 9);
  
  if(retVal != -1)  
    waitResponse();
}

void Imu::ekfReset()
{
  unsigned char writeBuffer[7] = {'s','n','p',0x95,0,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 5; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[5] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[6] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 7);
  
  if(retVal != -1)
    waitResponse();
}

void Imu::disableMagCorrection()
{
  unsigned char writeBuffer[8] = {'s','n','p',0x8D,1,0x03,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 6; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[6] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[7] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 8);
  
  if(retVal != -1)
    waitResponse();
}

void Imu::zeroRateGyros()
{
  unsigned char writeBuffer[7] = {'s','n','p',0x87,0,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 5; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[5] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[6] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 7);
  
  if(retVal != -1)
    waitResponse();
}

void Imu::selfTest()
{
  unsigned char writeBuffer[7] = {'s','n','p',0x88,0,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 5; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[5] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[6] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 7);
  
  if(retVal != -1)
    waitResponse();
}

void Imu::waitResponse()
{
  unsigned char buffer[1024];
  int numBytes = 0;
  int PT = 0x00;
  
  numBytes = readPacket(buffer);
  PT = buffer[0];
  
  while(PT == 0xB7)
  {
    numBytes = readPacket(buffer);
    PT = buffer[0];
  }
  
  printResponse(PT,buffer);
}

void Imu::autoSetAccelRef()
{
  unsigned char writeBuffer[7] = {'s','n','p',0x86,0,0x00,0x00};
  
  int writeCheckSum = 0;
  for(int i = 0; i < 5; i++)
    writeCheckSum += writeBuffer[i];
    
  writeBuffer[5] = (unsigned char)((writeCheckSum & 0xFF00)>>8);
  writeBuffer[6] = (unsigned char)(writeCheckSum & 0x00FF);
  
  int retVal = ftdi->writeData(writeBuffer, 7);
  
  if(retVal != -1)
    waitResponse();
}

int Imu::readPacket(unsigned char *buffer)
{
  int ret = 0,
      sum = 0,
      numDataBytes = 0;
      
  unsigned char buf[1] = {0};
  
  while(ret >= 0 && buf[0] != 's')
  {
    ret = ftdi->readData(buf, sizeof(buf));
    sum = buf[0];
  } 
  
  if(buf[0] == 's')
  {
    ret = ftdi->readData(buf, sizeof(buf));
    sum += buf[0];
    if(buf[0] == 'n')
    {
      ret = ftdi->readData(buf, sizeof(buf));
      sum += buf[0];
      if(buf[0] == 'p')
      {
        ret = ftdi->readData(buf, sizeof(buf));          
        sum += buf[0];        
        buffer[0] = buf[0];
        numDataBytes++;
        
        ret = ftdi->readData(buf, sizeof(buf));            
        sum += buf[0];
        int numBytes = buf[0];          
        buffer[1] = buf[0];
        numDataBytes++;
          
        unsigned char dataBuffer[numBytes];
        ret = ftdi->readData(dataBuffer, numBytes);
        for(int i = 0; i < numBytes; i++)
        {
          sum += dataBuffer[i];
          buffer[2+i] = dataBuffer[i];
          numDataBytes++;
        }  
                
        unsigned char checkSumBuf[2]; 
        ret = ftdi->readData(checkSumBuf, 2);             
        int checkSum = checkSumBuf[0];
        checkSum = checkSum << 8;
        checkSum = checkSum ^ checkSumBuf[1];
        if(checkSum == sum)
          //fprintf(stderr, "Check sum passed!\n");
          return numDataBytes;
        else
          //printf(stderr, "Check sum failed: %d != %d\n", checkSum, sum);
          return -1;
        
      }
    }
  }
  
  return -1;
}
